#include "nav2_util/node_utils.hpp"
#include <cmath>
#include <fstream>
#include "nav2_core/exceptions.hpp"
#include "planner_pkg/global_fixed_planner.hpp"
#include "planner_pkg/planner_util.hpp"
#include "agv_util/point_util.hpp"

namespace planner_pkg
{

    GlobalFixedPlanner::GlobalFixedPlanner()
        : tf_(nullptr), costmap_(nullptr)
    {
    }

    GlobalFixedPlanner::~GlobalFixedPlanner()
    {
        RCLCPP_INFO(
            logger_, "Destroying plugin %s of type GlobalFixedPlanner",
            name_.c_str());
    }
    void GlobalFixedPlanner::configure(
        const rclcpp_lifecycle::LifecycleNode::WeakPtr &parent, std::string name,
        std::shared_ptr<tf2_ros::Buffer> tf,
        std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
    {
        tf_ = tf;
        node_ = parent.lock();
        name_ = name;
        costmap_ = costmap_ros->getCostmap();
        global_frame_ = costmap_ros->getGlobalFrameID();

        clock_ = node_->get_clock();
        logger_ = node_->get_logger();
        // 创建导航规划
        planner_ = std::make_unique<planner_pkg::Fixed>(
            costmap_->getSizeInCellsX(),
            costmap_->getSizeInCellsY());
        // marker
        rviz_marker_ptr_ = std::make_shared<rviz_util_pkg::RvizMarker>(node_);
        // 从yaml 文件中加载点位
        points_ = agv_util::load_waypoint();
        updateWaypoints();
    }

    void GlobalFixedPlanner::cleanup()
    {
        RCLCPP_INFO(logger_, "正在清理类型为 GlobalFixedPlanner 的插件 %s",
                    name_.c_str());
        planner_.reset();
        rviz_marker_ptr_.reset();
    }

    void GlobalFixedPlanner::activate()
    {
        RCLCPP_INFO(logger_, "正在激活类型为 GlobalFixedPlanner 的插件 %s",
                    name_.c_str());
        if (show_marker_)
        {
            publish_marker();
        }
    }

    void GlobalFixedPlanner::deactivate()
    {
        RCLCPP_INFO(logger_, "正在停用类型为 GlobalFixedPlanner 的插件 %s",
                    name_.c_str());
    }

    nav_msgs::msg::Path
    GlobalFixedPlanner::createPlan(const geometry_msgs::msg::PoseStamped &start,
                                   const geometry_msgs::msg::PoseStamped &goal)
    {
        nav_msgs::msg::Path global_path;
        // Corner case of the start(x,y) = goal(x,y)
        if (start.pose.position.x == goal.pose.position.x &&
            start.pose.position.y == goal.pose.position.y)
        {
            RCLCPP_WARN(logger_, "规划器失败 createPlan start(x,y) = goal(x,y)");
            return global_path;
        }
        //  使用简单的 A* 算法规划路径
        if (!makePlan(start.pose, goal.pose, global_path))
        {
            RCLCPP_WARN(logger_, "规划全局路径失败");
            throw nav2_core::PlannerException("规划全局路径失败");
        }
        // 使用 costmap 检查该条路径是否经过障碍物
        for (geometry_msgs::msg::PoseStamped pose : global_path.poses)
        {
            unsigned int mx, my; // 将点的坐标转换为栅格坐标
            if (costmap_->worldToMap(pose.pose.position.x, pose.pose.position.y, mx, my))
            {
                unsigned char cost = costmap_->getCost(mx, my); // 获取对应栅格的代价值
                // 如果存在致命障碍物则抛出异常
                if (cost == nav2_costmap_2d::LETHAL_OBSTACLE)
                {
                    RCLCPP_WARN(logger_, "在(%f,%f)检测到致命障碍物，规划失败。", pose.pose.position.x, pose.pose.position.y);
                    throw nav2_core::PlannerException(
                        "无法创建目标规划: " + std::to_string(goal.pose.position.x) + "," + std::to_string(goal.pose.position.y));
                }
            }
        }
        return global_path;
    }
    /**
     * 从 start 到 goal 的路径，要求过一系列的点，这些点 pose_ 上的点，
     */
    bool GlobalFixedPlanner::makePlan(
        const geometry_msgs::msg::Pose &start,
        const geometry_msgs::msg::Pose &goal,
        nav_msgs::msg::Path &plan)
    {
        plan.poses.clear();
        plan.header.frame_id = global_frame_;
        plan.header.stamp = clock_->now();
        if (points_.empty())
        {
            RCLCPP_INFO(logger_, "points_ is empty,无法生成路径");
            return false;
        }
        double wx_start = start.position.x;
        double wy_start = start.position.y;
        unsigned int mx_start, my_start;
        if (!costmap_->worldToMap(wx_start, wy_start, mx_start, my_start))
        {
            RCLCPP_WARN(logger_, "无法规划路径，机器人起始位置不在全局代价地图上。");
            return false;
        }
        // 将起始点设置为可通行
        costmap_->setCost(mx_start, my_start, nav2_costmap_2d::FREE_SPACE);
        planner_->setMapSize(
            costmap_->getSizeInCellsX(),
            costmap_->getSizeInCellsY());
        planner_->setCostmap(costmap_->getCharMap());
        // 起始点
        int map_start[2];
        map_start[0] = mx_start;
        map_start[1] = my_start;
        // 目标点
        double wx_goal = goal.position.x;
        double wy_goal = goal.position.y;
        unsigned int mx_goal, my_goal;
        if (!costmap_->worldToMap(wx_goal, wy_goal, mx_goal, my_goal))
        {
            RCLCPP_WARN(logger_, "无法规划路径，目标点不在全局代价地图上。");
            return false;
        }
        // 检测目标点是否为致命障碍物
        unsigned char cost = costmap_->getCost(mx_goal, my_goal); // 获取对应栅格的代价值
        // 如果存在致命障碍物则抛出异常
        if (cost == nav2_costmap_2d::LETHAL_OBSTACLE)
        {
            RCLCPP_WARN(logger_, "在目标点(%f,%f)检测到致命障碍物，规划失败。", goal.position.x, goal.position.y);
            return false;
        }
        int map_goal[2];
        map_goal[0] = mx_goal;
        map_goal[1] = my_goal;
        planner_->setStart(map_start);
        planner_->setGoal(map_goal);
        // 使用 A* 规划
        auto start_time = std::chrono::high_resolution_clock::now();
        auto node_list = planner_->searchPath();
        if (node_list == nullptr || node_list->empty())
        {
            return false;
        }
        // RCLCPP_INFO(logger_, "node_list size=%ld", node_list->size());
        // 耗时 Path found
        auto end_time = std::chrono::high_resolution_clock::now();
        auto duration = std::chrono::duration_cast<std::chrono::microseconds>(end_time - start_time);
        RCLCPP_INFO(logger_, "Path found in %ld us", duration.count());

        // 将节点生成路径
        for (size_t index = 0; index < node_list->size() - 1; index++)
        {
            auto node_ptr = node_list->at(index);
            auto next_node_ptr = node_list->at(index + 1);
            RCLCPP_INFO_ONCE(logger_, "路径点 %s,%s", node_ptr->name.c_str(), next_node_ptr->name.c_str());
            // 循环次数
            double loop_count = std::hypot(node_ptr->x - next_node_ptr->x, node_ptr->y - next_node_ptr->y);
            double x_increment = (next_node_ptr->x - node_ptr->x) / loop_count;
            double y_increment = (next_node_ptr->y - node_ptr->y) / loop_count;
            unsigned int map_x, map_y;
            double world_x, world_y; // 将栅格坐标转换为世界坐标
            for (int i = 0; i < loop_count; i++)
            {
                map_x = node_ptr->x + i * x_increment;
                map_y = node_ptr->y + i * y_increment;
                costmap_->mapToWorld(map_x, map_y, world_x, world_y);
                // 生成pose
                geometry_msgs::msg::PoseStamped pose;
                pose.pose.position.x = world_x;
                pose.pose.position.y = world_y;
                pose.pose.position.z = 0.0;
                pose.pose.orientation.x = 0.0;
                pose.pose.orientation.y = 0.0;
                pose.pose.orientation.z = 0.0;
                pose.pose.orientation.w = 1.0;
                plan.poses.push_back(pose);
            }
        }
        //----------------------------------------------
        smoothApproachToGoal(goal, plan);
        return !plan.poses.empty();
    }

    void
    GlobalFixedPlanner::smoothApproachToGoal(
        const geometry_msgs::msg::Pose &goal,
        nav_msgs::msg::Path &plan)
    {
        // Replace the last pose of the computed path if it's actually further away
        // to the second to last pose than the goal pose.
        if (plan.poses.size() >= 2)
        {
            auto second_to_last_pose = plan.poses.end()[-2];
            auto last_pose = plan.poses.back();
            if (
                squared_distance(last_pose.pose, second_to_last_pose.pose) >
                squared_distance(goal, second_to_last_pose.pose))
            {
                plan.poses.back().pose = goal;
                return;
            }
        }
        geometry_msgs::msg::PoseStamped goal_copy;
        goal_copy.pose = goal;
        plan.poses.push_back(goal_copy);
    }

    bool GlobalFixedPlanner::updateWaypoints()
    {
        if (!costmap_)
        {
            RCLCPP_ERROR(logger_, " costmap is not initialized");
            return false;
        }
        auto point_nodes = std::make_shared<std::vector<std::shared_ptr<PointNodeFixed>>>();
        for (auto &point : points_)
        {
            unsigned int mx, my;
            if (!costmap_->worldToMap(point.pose.position.x, point.pose.position.y, mx, my))
            {
                RCLCPP_WARN(logger_, "无法规划路径，机器人路点 x=%f,y=%f 不在全局代价地图上。",
                            point.pose.position.x, point.pose.position.y);
                return false;
            }
            auto point_node = std::make_shared<PointNodeFixed>(
                static_cast<int>(mx),
                static_cast<int>(my),
                nullptr,
                0,
                0,
                point.name);
            point_nodes->push_back(point_node);
            // RCLCPP_INFO(logger_, "x=%d,y=%d", mx, my);
        }
        planner_->set_node_list(point_nodes);
        return true;
    }

    void GlobalFixedPlanner::publish_marker()
    {
        if (points_.empty())
        {
            return;
        }
        rclcpp::WallRate rate(5); // hz
        for (auto &point : points_)
        {
            double x = point.pose.position.x;
            double y = point.pose.position.y;
            // 发布
            rviz_marker_ptr_->publish_marker_point(x, y, point.name);
            rviz_marker_ptr_->publish_marker_txt(x, y, point.name);
            rate.sleep();
        }
    }
} // namespace planner_pkg

#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(planner_pkg::GlobalFixedPlanner,
                       nav2_core::GlobalPlanner)